#ifndef __CONTROLLEGUPDOWN__
#define __CONTROLLEGUPDOWN__

#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <stdio.h>
#include "PIDControler.h"

#define Gravity 9.8
#define SPRING_K 1000.0
#define MASS_LEG 3.0
#define axis_spring_leg_low 0.05

using namespace gazebo;
class ControlLegUpdown
{
  // 模型指针
  public: physics::ModelPtr model_leg;
  // spring_joint节点指针、
  public: physics::JointPtr spring_joint;
  // leg_low LINK指针
  public: physics::LinkPtr leg_low_link;
  // leg_high LINK指针
  public: physics::LinkPtr leg_high_link;
  //模拟弹簧
  PIDControler Spring_Leg;
  public:void Load(physics::WorldPtr world);
  //弹跳高度控制函数
  public:void Control_Height(double Height ,double Mass_Leg ,double Spring_K);

};

#endif
